#include <PID_v1.h>

#define LEFT_SENSOR 14
#define CENTER_SENSOR 16
#define RIGHT_SENSOR 18

#define MOTOR_A1 5
#define MOTOR_A2 6
#define MOTOR_B1 9
#define MOTOR_B2 10

double Setpoint = 0; // 目标值为0，表示车辆应当保持在中心位置
double Kp = 2;
double Ki = 5;
double Kd = 1;

double Input, Output;
PID pid(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

int leftSensorValue, centerSensorValue, rightSensorValue;

void setup() {
  Serial.begin(9600);
  pinMode(MOTOR_A1, OUTPUT);
  pinMode(MOTOR_A2, OUTPUT);
  pinMode(MOTOR_B1, OUTPUT);
  pinMode(MOTOR_B2, OUTPUT);
  pinMode(CENTER_SENSOR, INPUT); // 设置中心传感器为输入模式

  pid.SetMode(AUTOMATIC);
  pid.SetOutputLimits(-255, 255);

  Serial.println("PID Tuning:");
  Serial.print("Kp: ");
  Serial.println(Kp);
  Serial.print("Ki: ");
  Serial.println(Ki);
  Serial.print("Kd: ");
  Serial.println(Kd);
}

void loop() {
    while(1){
          // 计算传感器读数的误差，以便作为PID控制器的输入
        double error = (double)rightSensorValue - (double)leftSensorValue;
        Input = error / 1023.0;
          pid.Compute();
            // 使用PID输出来控制电机转向幅度
  int leftSpeed = constrain(200 + Output, 0, 255); // 左轮速度
  int rightSpeed = constrain(200 - Output, 0, 255); // 右轮速度
    // 输出调试信息
  Serial.print("Input: ");
  Serial.print(Input);
  Serial.print(" Output: ");
  Serial.println(Output);
  
    if(!(digitalRead(16))){
        // 控制右轮转速增加，左转
        analogWrite(MOTOR_A1, 0);
        digitalWrite(MOTOR_A2, leftSpeed);
        analogWrite(MOTOR_B1, rightSpeed);
        digitalWrite(MOTOR_B2, LOW);
        // spin(10,155);
        // delay(100);
    }
    else if(!(digitalRead(17))){
        // 控制左轮转速增加，右转
             motorA1Value = leftSpeed;
            motorA2Value = LOW;
            motorB1Value = LOW;
            motorB2Value = rightSpeed;
        
        // spin(155,10);
        // delay(100);
    }
    else{
        // 左右轮转速恢复正常
    analogWrite(MOTOR_A1, leftSpeed);
    digitalWrite(MOTOR_A2, LOW);
    analogWrite(MOTOR_B1, rightSpeed);
    digitalWrite(MOTOR_B2, LOW);
    }
      delay(10);
    }






}
void run() {
    if(!(digitalRead(16))){
        // 控制右轮转速增加，左转
        analogWrite(MOTOR_A1, 0);
        digitalWrite(MOTOR_A2, leftSpeed);
        analogWrite(MOTOR_B1, rightSpeed);
        digitalWrite(MOTOR_B2, LOW);
        // spin(10,155);
        // delay(100);
    }
    else if(!(digitalRead(17))){
        // 控制左轮转速增加，右转
             motorA1Value = leftSpeed;
            motorA2Value = LOW;
            motorB1Value = LOW;
            motorB2Value = rightSpeed;
        
        // spin(155,10);
        // delay(100);
    }
    else{
        // 左右轮转速恢复正常
    analogWrite(MOTOR_A1, leftSpeed);
    digitalWrite(MOTOR_A2, LOW);
    analogWrite(MOTOR_B1, rightSpeed);
    digitalWrite(MOTOR_B2, LOW);
    }
}
//调节转速函数，xy两个引脚，speed控制转速
void spin(int x,int y)
{
  analogWrite( 9 , x );
  analogWrite( 5 , y );
}